#include <ros/ros.h>

#include "serial.hpp"
#include "cg_interfaces/ControlSignal.h"

#include "encoder.hpp"

const int bufferSize = 12;

bool TryGetParam(std::string &port, int &baudrate)
{
    ros::NodeHandle nh("~");
    if (!nh.getParam("port", port))
    {
        ROS_ERROR("Failed to get port parameter");
        return false;
    }
    if (!nh.getParam("baudrate", baudrate))
    {
        ROS_ERROR("Failed to get baudrate parameter");
        return false;
    }
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rc_controller");

    unsigned char buffer_rec[bufferSize];
    int count = 0;
    cg_interfaces::ControlSignal signal;

    
    std::string port;
    int baudrate;
    if (!TryGetParam(port, baudrate))
    {
        ROS_ERROR("Failed to get parameters");
        return -1;
    }


    CG::Serial serial;
    if (!serial.Open(port, baudrate))
    {
        ROS_ERROR("Failed to open serial port");
        return -1;
    }
    
    ros::NodeHandle nh;
    ros::Publisher signal_pub = nh.advertise<cg_interfaces::ControlSignal>("control_signal", 1);

    ros::Rate rate(10);
    while (ros::ok())
    {
        rate.sleep();
        while (true)
        {
            int read = serial.Read(buffer_rec + count, bufferSize - count);
            if (read <= 0)
            {
                break;
            }
            count += read;
            if (count >= bufferSize)
            {
                count = 0;
                switch (buffer_rec[2])
                {
                case 0x01:
                    DecodeControlFrame(buffer_rec, signal.linear_velocity, signal.brake_precent, signal.angular_velocity);
                    signal_pub.publish(signal);
                    break;
                case 0x02:
                    DecodeHighIOFrame(buffer_rec, signal.park);
                    signal_pub.publish(signal);
                    break;
                case 0x03:
                    break;
                default:
                    break;
                }
            }
        }
        ros::spinOnce();
    }
    return 0;
}